package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;

/**
 * This is a common sequence used by FindingTreasureSequence and FindingMazeSequence.
 * 
 * When the robot failed to find a treasure or a maze by turning around, this sequence
 * will be used to find which direction the robot should make a move.
 * 
 * @author XWu1
 *
 */
public class FindingStepSequence implements IStateMachine {
	public static final int TURN_DEGREES = 15;

	public enum State implements IState {
		Initial {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				machine.changeState(Detect);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Detect {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				//IDetector outerWallDetector = robot.getOuterWallDetector();
				//IDetector obstacleDetector = robot.getObstacleDetector();
				IDetector targetDetector = m_sequence.getTargetDetector();
				
				IRobotSensors sensor = robot.getSensor();
				
				if (sensor.detectFrontWall()) {
					machine.changeState(Blocked);
				}
				else if (targetDetector.isPresent()) {
					machine.changeState(TargetFound);
				}
				else {
					machine.changeState(Unblocked);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Blocked {
			private int m_turnedDegree = 0;
			
			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Blocked", 4);
				m_turnedDegree = 0;
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				IRobotSensors sensor = robot.getSensor();
				
				IDetector targetDetector = m_sequence.getTargetDetector();
				
				if (targetDetector.isPresent()) {
					machine.changeState(TargetFound);
				}
				else if (sensor.detectFrontWall()) {
					if (!sensor.detectLeftWall()) {
						robot.turnLeft();
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					}
					else if (!sensor.detectRightWall()) {
						robot.turnRight();
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					}
					else {
						robot.turnBack();
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					}
					machine.changeState(Detect);
				}
				else {
					machine.changeState(Unblocked);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Unblocked {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
			}

			@Override
			public void exit(IStateMachine machine) {
			}
			
		},
		TargetFound {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
			}

			@Override
			public void exit(IStateMachine machine) {
			}
			
		};

		static FindingStepSequence m_sequence;
		
		public static void setSequence(FindingStepSequence sequence) {
			m_sequence = sequence;
		}
	}
	
	/**
	 * Indicate if the target is to find a treasure or a maze
	 * @author XWu1
	 *
	 */
	public enum Target {
		Treasure,
		Maze
	}
	
	RobotRunningSequence m_parentSequence;
	Target m_target;
	IState m_currState, m_nextState;
	
	public FindingStepSequence(RobotRunningSequence parentSequence, Target target) {
		m_parentSequence = parentSequence;
		m_target = target;
		
		State.setSequence(this);
		
		m_currState = State.Initial;
	}
	
	public Target getTarget() {
		return m_target;
	}
	
	public IDetector getTargetDetector() {
		switch (m_target) {
		case Treasure:
			return robot().getTreasureDetector();
			
		case Maze:
			return robot().getMaze1Detector();
			
			default:
				return null;
		}
	}
	
	public void start() {
		Robot robot = m_parentSequence.robot();
		robot.enableTreasureDetector(true);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(true);
		robot.enableMaze2Detector(true);
		robot.enableMaze3Detector(true);
		robot.enableMazeEntranceGuideDetector(false);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(true);
	}
	
	@Override
	public void changeState(IState state) {
		if (!state.equals(m_currState)) {
			m_currState.exit(this);
			m_currState = state;
			m_currState.enter(this);
		}
	}

	@Override
	public IState getState() {
		return m_currState;
	}

	@Override
	public Robot robot() {
		return m_parentSequence.robot();
	}

	@Override
	public void waitForResume() {
		m_parentSequence.waitForResume();
	}

	public boolean succeeded() {
		return m_currState.equals(State.Unblocked);
	}
	
	public boolean targetFound() {
		return m_currState.equals(State.TargetFound);
	}

	@Override
	public void move() {
		if (m_nextState != null) {
			changeState(m_nextState);
			m_nextState = null;
		}
		
		m_currState.move(this);
	}

	@Override
	public void reset() {
		m_nextState = State.Initial;
	}
}
